#include "HandManipulation.h"
#include "SceneProcessorHelper.h"

HandManipulation::HandManipulation(void)
{
}


HandManipulation::~HandManipulation(void)
{
}

void HandManipulation::SingleHandMani( Point3D lastPos,Point3D curPos,RSTType & rstType )
{
	// SINGLE HAND
	Point3D lastPosOgl,curPosOgl;
	if((SceneProcessorHelper::getInstance()->ConvertToOpengl(lastPos,lastPosOgl))
		&& (SceneProcessorHelper::getInstance()->ConvertToOpengl(curPos,curPosOgl))){
			std::cout << "\nogl data :\n\tlast:" << lastPosOgl << "\tcurrent:" << curPosOgl;
			convertToRST(lastPosOgl,curPosOgl,SINGLE_HAND,rstType);
	}	
}

void HandManipulation::convertToRST( Point3D lastpos,Point3D curpos,int hand,RSTType & rstType )
{
	if(hand == SINGLE_HAND){
		rstType.g_movement = curpos - lastpos;
		rstType.g_rstType = TRANSLATE;
	}
}

void HandManipulation::convertToRST( int convertType,Point3D lastpos[2],Point3D curpos[2],int hand,RSTType & rstType )
{
	if(convertType == CAMERA_CTRL)
		convertToRSTCamera(lastpos,curpos,hand,rstType);
	else if(convertType == SELECT_RECOG)
		convertToRSTObject(lastpos,curpos,hand,rstType);
}

void HandManipulation::convertToRSTCamera( Point3D lastpos[2],Point3D curpos[2],int hand,RSTType & rstType )
{
	if(hand == DOUBLE_HAND){
		// rotate
		Point3D curLine = curpos[1] - curpos[0],
			lastLine = lastpos[1] - lastpos[0],
			curLineXOZ = Point3D(curLine.x,0,curLine.z),
			lastLineXOZ = Point3D(lastLine.x,0,lastLine.z);
		if((curLineXOZ.length() != 0) || (lastLineXOZ.length() != 0)){
			rstType.g_rotateAxis = Point3D(0,1,0);
			rstType.g_rotateAngle = acos(curLineXOZ.dotProduct(lastLineXOZ)/curLineXOZ.length()/lastLineXOZ.length());
			rstType.g_rotateAngle *= 10;
			rstType.g_rstType |= ROTATE;
		}

		// split the angle to hor and ver
		Point3D deltapos0 = curpos[0] - lastpos[0];
		Point3D deltapos1 = curpos[1] - lastpos[1];
		double angle = 0;
		if((deltapos0 != Point3D(0,0,0) && (deltapos1 != Point3D(0,0,0)))){
			double angleHelp = deltapos0.dotProduct(-deltapos1)/deltapos0.length()/deltapos1.length();
			if(fabs(angleHelp) >= 1)
				angle = 0;
			else
				angle = acos(angleHelp)/2;
		}		
		std::cout << "\tsplit angle:" << angle << "\t";
		// translate part
		rstType.g_movement = (deltapos0+deltapos1)*sin(angle)/2;
		rstType.g_rstType |= TRANSLATE;
		rstType.g_movement /= 10;

		// scale part
		Point3D scaleP = (deltapos0-deltapos1)*cos(angle)/2;
		scaleP.scale(lastpos[1] - lastpos[0]);
		double lastDis = (lastpos[1] - lastpos[0]).length(),
			curDis = (curpos[1] - curpos[0]).length();
		if(lastDis > curDis){		
			scaleP = -abs(scaleP);
		}
		else{
			scaleP = abs(scaleP);
		}
		rstType.g_scale = scaleP;
		rstType.g_rstType |= SCALE;
	}
}

void HandManipulation::DoubleHandMani( int type,Point3D lastPos[2],Point3D curPos[2],RSTType & rstType )
{
	Point3D lastPosOgl[2],curPosOgl[2]; 
	if((SceneProcessorHelper::getInstance()->ConvertToOpengl(lastPos[0],lastPosOgl[0]))
		&& (SceneProcessorHelper::getInstance()->ConvertToOpengl(lastPos[1],lastPosOgl[1]))
		&& (SceneProcessorHelper::getInstance()->ConvertToOpengl(curPos[0],curPosOgl[0]))
		&& (SceneProcessorHelper::getInstance()->ConvertToOpengl(curPos[1],curPosOgl[1]))){
			std::cout << "\nogl data: left move from " << lastPosOgl[0] << " to " << curPosOgl[0]
			<< "\t right move from " << lastPosOgl[1] << " to " << curPosOgl[1];
			convertToRST(type,lastPosOgl,curPosOgl,DOUBLE_HAND,rstType);
	}
}

void HandManipulation::convertToRSTObject( Point3D lastpos[2],Point3D curpos[2],int hand,RSTType & rstType )
{
	time_t temp,curTime = clock();
	if(hand == DOUBLE_HAND){
		// rotate
		Point3D curLine = curpos[1] - curpos[0],
			lastLine = lastpos[1] - lastpos[0];
		float curLineLength = curLine.length(),
			lastLineLength = lastLine.length();
		if((curLineLength != 0) && (lastLineLength != 0)){
			rstType.g_rotateAxis = lastLine.crossProduct(curLine);
			rstType.g_rotateAngle = acos(curLine.dotProduct(lastLine)/curLineLength/lastLineLength);
			//rstType.g_rotateAngle *= 5;
			rstType.g_rstType |= ROTATE;
		}

		// split the angle to hor and ver
		Point3D deltapos0 = curpos[0] - lastpos[0];
		Point3D deltapos1 = curpos[1] - lastpos[1];
		double angle = 0;
		if((deltapos0.length() != 0) && (deltapos1.length() != 0)){
			double angleHelp = deltapos0.dotProduct(-deltapos1)/deltapos0.length()/deltapos1.length();
			if(angleHelp >= 1)
				angle = 0;
			else if(angleHelp <= -1)
				angle = M_PI_2;
			else
				angle = acos(angleHelp)/2;
		}		
		std::cout << "\tsplit angle:" << angle << "\t";
		// translate part
		std::cout << "\ndeltapos0:" << deltapos0 << "\tdeltapos1:" << deltapos1 << "\tlastLineLength:" << lastLineLength;
		rstType.g_movement = (deltapos0+deltapos1)*sin(angle)/2;
		rstType.g_rstType |= TRANSLATE;

		// scale part
		Point3D scaleP = (deltapos0-deltapos1)*cos(angle)/2;
		scaleP.scale(lastLineLength);
		if(lastLineLength > curLineLength){		
			scaleP = -abs(scaleP);
		}
		else{
			scaleP = abs(scaleP);
		}
		rstType.g_scale = scaleP;
		rstType.g_rstType |= SCALE;
	}
	if(TIME_COUNT){
		temp = curTime;
		curTime = clock();
		std::cout << "\n\t\t\tdouble manipulate convert:" << curTime - temp;
	}
}
